// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/nav_sat_status.hpp"
#include "basic_sprayer_interfaces/msg/nav_sta_dp.hpp"

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>

using namespace std::chrono_literals;

/* 差分GPS 信息发布 */

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class dGPSPublisher : public rclcpp::Node
{
public:
    dGPSPublisher()
        : Node("dgps_publisher")
    {
        fd = 0;
        if ((fd = open_port((char *)"/dev/gps_0")) < 0)
        {
            printf("GNSS open failed\n");
            /* 设置错误位 */
        }
        else
        {
            set_opt(fd, 115200, 8, 'N', 1);
        }
        /* 连接了GPS */
        publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::NavStaDP>("dgps", 10);
    }

    void dgpsProcess(void)
    {
        /*  在这里处理  */
        char buffer[1024] = {0}; // 设置保存数据的buff大小
        int read_num;
        char *p = 0;
        if (fd >= 0)
        {
            read_num = read(fd, buffer, 1024); // 阻塞读取到空格。
            printf(buffer);
            (void)read_num;
            /* 在这里解析经纬度 */
            message.header.stamp = this->now();
            message.header.frame_id = "earth"; // 暂定

            p = strstr(buffer, "$KSXT");
            if (p == NULL)
            {
                goto fail;
            }
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            /* 时间 */
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.longitude = atof(p); // 经度
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.latitude = atof(p); // 纬度
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.altitude = atof(p); //  高度
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            double yaw = atof(p) - 90;
            if (yaw < 0)
            {
                yaw = yaw + 360;
            }
            if (yaw > 180)
            {
                yaw = yaw - 360;
            }
            message.yaw = -yaw;
            // message.yaw = atof(p);       //  方向角 0 ～ 360°
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.pitch = atof(p); //  俯仰
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.speed_angle = atof(p); //  速度角
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.speed = atof(p); //  速度  km/h
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.roll = atof(p); //  横滚  -90°~90°
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            int state = atoi(p);
            // if(state > 1){
            //     message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX;
            // }else if(state > 0) {
            //     message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
            // }else{
            //     message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
            // }
            // message.yaw = atof(p);     //  卫星定位状态
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            state = atoi(p);
            if (state > 1)
            {
                message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX;
            }
            else if (state > 0)
            {
                message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
            }
            else
            {
                message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
            }
            // message.yaw = atof(p);   //  卫星定向状态
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            // message.yaw = atof(p);   //  前天线当前参与解算的卫星数量
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            // message.yaw = atof(p);   //  后天线当前参与解算的卫星数量
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.base_east_distance = atof(p); //  东向位置坐标   单位为m
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.base_north_distance = atof(p); //  北向位置坐标  单位为m
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.base_up_distance = atof(p); //  天向位置坐标  单位为m
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.base_east_speed = atof(p); //  东向速度，km/h
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.base_north_speed = atof(p); //  北向速度，km/h
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            message.base_up_speed = atof(p); //  天向速度，km/h
            p = ship_one_comma(p);
            if (p == NULL)
            {
                goto fail;
            }
            // message.yaw = atof(p);   //  差分龄期
            if (p == NULL)
            {
                goto fail;
            }
            // message.yaw = atof(p);   //  基准站卫星数
        }
        else
        {
            message.header.stamp = this->now();
            message.header.frame_id = "earth";                                     // 暂定
            message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; // 未定位
            sleep(1);                                                              //
        }
        publisher_->publish(message);
        return;
    fail:                                                                      // 失败
        message.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; // 未定位
        publisher_->publish(message);
    }

private:
    rclcpp::Publisher<basic_sprayer_interfaces::msg::NavStaDP>::SharedPtr publisher_;
    basic_sprayer_interfaces::msg::NavStaDP message;
    int fd;
    char *ship_one_comma(char *now_p)
    {
        char *p = NULL;
        p = strstr(now_p, ",");
        if (p != NULL)
        {
            p++;
            return p;
        }
        return NULL;
    }
    // void gpsKSXT(uint8_t *_ucaBuf, uint16_t _usLen)
    // {
    // 	char *p;

    // 	p = (char *)_ucaBuf;
    // 	p[_usLen] = 0;

    // 	/* �ֶ�1 UTCʱ�䣬hhmmss.sss��ʽ */
    // 	p = strchr(p, ',');
    // 	if (p == 0)
    // 	{
    // 		return;
    // 	}
    // 	p ++;
    // 	/* 这中间是时间，跳过 */
    // 	p = strchr(p, ',');
    // 	p ++;
    // 	/* 经度 */
    // 	g_tGPS.longitude = atof(p);
    // 	p = strchr(p, ',');
    // 	p++;
    // 	/* 纬度 */
    // 	g_tGPS.latitude = atof(p);
    // 	p = strchr(p, ',');
    // 	p++;
    // 	/* 高度 */
    // 	g_tGPS.height = atof(p);
    // 	p = strchr(p, ',');
    // 	p++;
    // 	/* 方向角 */
    // 	g_tGPS.Yaw = atof(p);
    // 	/* 到此结束，后面不在解析 */

    // }

    /* 打开串口 */
    int open_port(char *uartname)
    {
        //    int fd = open(uartname, O_RDWR|O_NOCTTY|O_NONBLOCK);
        int fd = open(uartname, O_RDWR | O_NOCTTY);
        if (-1 == fd)
        {
            perror("Can't Open Serial Port");
            return (-1);
        }
        /*恢复串口为阻塞状态*/
        // if(fcntl(fd, F_SETFL, 0)<0)
        // {
        //     printf("fcntl failed!\n");
        // }else{
        //     printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));  //直接设置成0就可以，虽然有点不符合规范，但是是可行的。此时是只读的
        // }
        /*测试是否为终端设备*/
        if (isatty(STDIN_FILENO) == 0)
        {
            printf("standard input is not a terminal device\n");
        }
        else
        {
            printf("isatty success!\n");
        }
        printf("fd-open=%d\n", fd);
        return fd;
    }

    int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
    {
        struct termios newtio, oldtio;
        if (tcgetattr(fd, &oldtio) != 0)
        {
            perror("SetupSerial 1");
            return -1;
        }
        bzero(&newtio, sizeof(newtio));
        newtio.c_cflag |= CLOCAL | CREAD;
        //    newtio.c_cflag  |=  CREAD;
        newtio.c_cflag &= ~CSIZE;

        switch (nBits)
        {
        case 7:
            newtio.c_cflag |= CS7;
            break;
        case 8:
            newtio.c_cflag |= CS8;
            break;
        }

        switch (nEvent)
        {
        case 'O':
            newtio.c_cflag |= PARENB;
            newtio.c_cflag |= PARODD;
            newtio.c_iflag |= (INPCK | ISTRIP);
            break;
        case 'E':
            newtio.c_iflag |= (INPCK | ISTRIP);
            newtio.c_cflag |= PARENB;
            newtio.c_cflag &= ~PARODD;
            break;
        case 'N':
            newtio.c_cflag &= ~PARENB;
            break;
        }

        switch (nSpeed)
        {
        case 2400:
            cfsetispeed(&newtio, B2400);
            cfsetospeed(&newtio, B2400);
            break;
        case 4800:
            cfsetispeed(&newtio, B4800);
            cfsetospeed(&newtio, B4800);
            break;
        case 9600:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
        case 115200:
            cfsetispeed(&newtio, B115200);
            cfsetospeed(&newtio, B115200);
            break;
        case 460800:
            cfsetispeed(&newtio, B460800);
            cfsetospeed(&newtio, B460800);
            break;
        case 921600:
            printf("B921600\n");
            cfsetispeed(&newtio, B921600);
            cfsetospeed(&newtio, B921600);
            break;
        default:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
        }
        if (nStop == 1)
            newtio.c_cflag &= ~CSTOPB;
        else if (nStop == 2)
            newtio.c_cflag |= CSTOPB;
        newtio.c_cc[VTIME] = 0;
        newtio.c_cc[VMIN] = 1;
        newtio.c_lflag |= ICANON;
        tcflush(fd, TCIFLUSH);
        if ((tcsetattr(fd, TCSANOW, &newtio)) != 0)
        {
            perror("com set error");
            return -1;
        }
        // printf("set done!\n\r");
        return 0;
    }
};

/* 控制退出 */
int inth = 1;
void mySigintHander(int sig)
{
    (void)sig;
    inth = 0;
}

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto pub_node = std::make_shared<dGPSPublisher>();
    signal(SIGINT, mySigintHander);
    while (inth)
    { // 执行gps的解码并且 发送定位坐标
        pub_node->dgpsProcess();
    }
    rclcpp::shutdown();
    return 0;
}
